// auction_matching_kdtree.cpp
// 04/20/2011 : created

#include "auction_matching_kdtree.h"
#include "Log.h"
#include <queue>

void AuctionMatchingKDTree::solve()
{
  assert(n1() == n2());
  n = n1();
  m_p = std::vector<double>(n, 0);  // init p(y) = 0, for all y \in Y

  if (m_refTree) delete m_refTree;
  m_refTree = new KDTreePrice(m_point2, m_p);
  Log::m_log << "Building" << endl;
  m_refTree->buildTree();
  Log::m_log << "Tree built" << endl;

  double s = 0;
  for (int y = 0; y < n; y++) s += m_refTree->price(y);
  Log::m_log << "Sum of prices = " << s << endl;

  //for (double epsilon = 0.1; epsilon >= 0.1; epsilon /= 5) {
  for (double epsilon = 1.0/n/*n/10.0*/; epsilon >= 1.0/n || epsilon >= 1e-3; epsilon /= 5) {
    std::queue<int> bidders;
    m_match = std::vector<int>(2*n, -1);  // init M = \emptyset, keep prices from previous iteration
    for (int i = 0; i < n; i++) bidders.push(i);
    int match_size=0;
    Log::m_log << "---- epsilon = " << epsilon << " ----" << endl;
    double pruned = 0;
    while (!bidders.empty()) {
      int x = bidders.front(); bidders.pop();

      std::vector<int> knnIdx(2, -1);
      std::vector<double> minBound(2, POSITIVE_INFINITY);
      pruned += m_refTree->findKNN(m_point1[x], knnIdx, minBound);

      double v = minBound[0], w = minBound[1];
      int min_y = knnIdx[0];
      m_refTree->set_price(min_y, m_refTree->price(min_y) + w - v + epsilon);
      min_y += n;   // add n to encode right node index

      //Log::m_log << "knn done v=" << v << " w=" << w << " x,y=" << x << "," << min_y << " p[y]=" << m_refTree->price(min_y-n) << endl;

      if (m_match[min_y] != -1) {
        bidders.push(m_match[min_y]);
        m_match[m_match[min_y]] = -1;
        //Log::m_log << "push " << m_match[min_y] << " "; Log::m_log.flush();
      }
      else {
        match_size++;
        if (match_size%100 == 0) Log::m_log << "size=" << match_size << " pruned=" << pruned << endl;
      }
      m_match[x] = min_y;
      m_match[min_y] = x;
    }
    Log::m_log << "pruned=" << pruned << endl;
  }
  for (int x = 0; x < n; x++) {
    int yn = m_match[x], y = yn-n, origY = m_refTree->originalIdx(y)+n;
    m_match[x] = origY;
    m_match[origY] = x;
  }

  s = 0;
  for (int y = 0; y < n; y++) s += m_refTree->price(y);
  Log::m_log << "Sum of prices = " << s << endl;

  Log::m_log << endl << "cost=" << cost() << endl;
}

double KDTreePrice::implementKNN(const vec& query, std::vector<int>& knnIdx, std::vector<double>& minBound) 
{
  double lowerBound = m_box.lowerBound(query)+m_minPrice;
  //Log::m_log << "lowerBound=" << lowerBound << endl;
  if (lowerBound >= minBound.back()) {               // pruned this node
    //Log::m_log << "pruned" << endl;
    return n();
  }

  if (m_child.size() == 0) {
    int K = knnIdx.size();
    //Log::m_log << "reach child ms=" << m_s << " me=" << m_e << endl;
    //for_all_node_data(i) {
    //  Log::m_log << "p[" << i << "]=" << price(i) << endl;
    //}
    for_all_node_data(i) {
      double dist = arma::norm(query-get(i), 2)+price(i);
      for (int k = 0; k < K; k++) if (dist < minBound[k]) {
        knnIdx.insert(knnIdx.begin()+k, m_s+i); knnIdx.pop_back();
        minBound.insert(minBound.begin()+k, dist); minBound.pop_back();
        //Log::m_log << "i=" << m_s+i << " (" << get(i)[0] << "," << get(i)[1] << ")" << " dist=" << dist << endl;
        break;
      }
    }
    return 0;
  }
  else {
    //Log::m_log << "reach  node ms=" << m_s << " me=" << m_e << " nc = " << m_child.size() << endl;
    std::vector<KDTreePrice*> childs(2);
    childs[0] = child(0);
    childs[1] = child(1);
    if (childs[0]->m_box.lowerBound(query)+childs[0]->m_minPrice > 
        childs[1]->m_box.lowerBound(query)+childs[1]->m_minPrice) {
      childs[0] = child(1);
      childs[1] = child(0);
      //Log::m_log << "change node order" << endl;
    }
    double s = 0;
    for (unsigned int i = 0; i < m_child.size(); i++) {
      KDTreePrice* c = child(i);
      s += c->implementKNN(query, knnIdx, minBound);
    }
    return s;
  }
}

void KDTreePrice::set_price(int i, double price) 
{ 
  int idx = m_s+i;
  m_price[idx] = price;
  KDTreePrice *p = NULL, *q = this;

  // find the leaf node that contains idx
  p = findLeaf(idx);
  assert(p->m_child.size() == 0);
  p->updatePrice();
}

KDTreePrice* KDTreePrice::findLeaf(int idx)
{
  for (int i = 0; i < (int) m_child.size(); i++) {
    if (child(i)->m_s <= idx && child(i)->m_e > idx)
      return child(i)->findLeaf(idx);
  }
  return this;
}

void KDTreePrice::updatePrice() {
  // go up the tree, update minPrice, maxPrice
  m_minPrice = POSITIVE_INFINITY;
  m_maxPrice = NEGATIVE_INFINITY;
  if (m_child.size() == 0) {
    for_all_node_data(i) {
      double p = price(i);
      if (m_minPrice > p) m_minPrice = p;
      if (m_maxPrice < p) m_maxPrice = p;
    }
  }
  else {
    for (int i = 0; i < (int) m_child.size(); i++) {
      KDTreePrice *q = child(i);
      if (m_minPrice > q->m_minPrice) m_minPrice = q->m_minPrice;
      if (m_maxPrice < q->m_maxPrice) m_maxPrice = q->m_maxPrice;
    }
  }
  //Log::m_log << "ms=" << m_s << " me=" << m_e << " minP=" << m_minPrice << " maxP=" << m_maxPrice << endl;
  if (m_parent)
    static_cast<KDTreePrice*>(m_parent)->updatePrice();
}

